classdef  feedforwardControler <handle
    %这个是累加式pid控制器plus，增加前馈
    properties
        u_now;     % 控制量，成为真正的控制输出
        ref_value;   %这个就是参考值，然后减去实际值就是err
        err_sum;    %求和的误差，用于增量式
        err_last;
        
        %so-called params
        kp;
        kv;
        tv;
        tm;
        km;
        kd;
        kt;
        
        is_torque_feedforward_enable;
        n_group;
    end
    
    methods
        function obj = feedforwardControler(sj_obj,motor_obj,is_torque_feedforward_enable,ts,zeta)

            if(nargin >= 2 ) 
                %先用sj初始化所有参数
                if(nargin == 5)
                    if (zeta == 1), wn = 4.75/ts; end % 临界阻尼系统的自然频率
                    if (zeta < 1), wn = 3.5/(zeta*ts); end % 欠阻尼系统的自然频率
                    if (zeta > 1), wn = 3.3/((zeta- sqrt(zeta^2-1))*ts); end % 过阻尼系统的自然频率
                else
                    %使用默认的特性
                    ts = 0.1;%调节时间
                    zeta = 1;%阻尼比
                    wn = 4.75/ts;
                end
                %以下全部使用力矩模式的值，因为题目如此要求
                obj.kt = motor_obj.Ka*motor_obj.Kg;
                obj.tm = sj_obj.f_Im./sj_obj.Bm; % 用理论等效惯量计算
                obj.km = obj.kt./sj_obj.Bm;
                obj.kd = 1./sj_obj.Bm;
                
                obj.tv = obj.tm;
                obj.kv = 2*zeta*wn./obj.km;
                obj.kp = wn/(2*zeta);
                
                obj.n_group = sj_obj.n_group;
                disp("using default attribute");
                if(nargin == 2)
                    obj.is_torque_feedforward_enable = zeros(obj.n_group,1);
                elseif(nargin == 3)
                    obj.is_torque_feedforward_enable = is_torque_feedforward_enable;
                end
            else
                ME = MException('need at least 3 pid param!');
                throw(ME)
            end
            [obj.u_now,obj.err_last,obj.err_sum] = deal(zeros(obj.n_group,1));
%             if(nargin == 4)
%                 obj.n_group = n_group;
%                 obj.kp = kp;
%                 obj.ki = ki;
%                 obj.kd = kd;
%                 disp("using default value 0");
%                 obj.ref_value = zeros(n_group,3);%z这里是变化的，因为考虑到要用速度和加速度的参考值
%             elseif(nargin == 5)
%                 obj.n_group = n_group;
%                 obj.ref_value = ref_value;
%                 obj.kp = kp;
%                 obj.ki = ki;
%                 obj.kd = kd;
%                 disp("initial fully");
%             else
%                 ME = MException('need at least 3 pid param!');
%                 throw(ME)
%             end
%             [obj.u_now,obj.err_last,obj.err_sum] = deal(zeros(n_group,3));

        end
        
        
        function con_value = get_con_value(obj,actual_value,simu_period)                        
            %这个是获得累加式的控制值，每次要传入state-info，也就是三种量
            con_value = zeros(obj.n_group,1,4);%组数、一行、con, e_pos, err_vel,e_sum
            err_now = obj.ref_value - actual_value;

            %注意这里的err-sum不是真的单纯期望减误差的累积！,而是err-vel的积分
            
            %先做位置P
            bad_rows = find(err_now(:,1)< -pi,obj.n_group);
            err_now(bad_rows,1) = mod(err_now(bad_rows,1),pi);
            bad_rows = find(err_now(:,1)> pi,obj.n_group);
            err_now(bad_rows,1) = mod(err_now(bad_rows,1),-pi);
            
            %con始终为列向量
            pos_con = obj.kp .* err_now(:,1);
            
            vel_err_now = pos_con+err_now(:,2);
            obj.err_sum = obj.err_sum + vel_err_now * simu_period;
            
            obj.u_now = obj.kv.*obj.tv .* vel_err_now + obj.kv .* obj.err_sum;       
            %还需要增加前馈部分
            obj.u_now = obj.u_now + obj.ref_value(2)./obj.km+ obj.ref_value(3).*obj.tm./obj.km;
            
            
            con_value(:,1,3) =  obj.err_last;
            obj.err_last = vel_err_now;
            con_value(:,1,1) = obj.u_now;
            con_value(:,1,2) = vel_err_now;
            con_value(:,1,4) = obj.err_sum;
            
            %还没考虑微分，以及加不加什么力矩前馈！！！！！！！！！！
        end
        
        function pid_reset(obj)
            [obj.u_now,obj.err_last,obj.err_sum] = deal(zeros(obj.n_group,1));
        end
        
        
        %一定要记得每次循环时把轨迹的目标发过来，一行就可以
        function set_target(obj,ref_value)
            obj.ref_value = ref_value;            
        end
    end
     
end